//
//  simController.m
//  roversimulator
//
//  Created by Matt Roman on 8/11/08.
//  Copyright 2008 University of Oklahoma. All rights reserved.
//

#import "simController.h"

#define FARCLIPPING		1001.0
#define NEARCLIPPING	0.2

#define WHEELWIDTH		0.1
#define WHEELBASE		0.6286
#define WHEELTRACK		0.59
#define SMALL_NUM		0.0001	// small angle check for parallelism between ray and triangle
#define GROUNDSCALE		1		// ground unit scaling factor (unit/m)
#define HEIGHTSCALE		3		// verticle height scaleing factor

GLfloat lightAmbient[]={0.5f,0.5f,0.5f,1.0f};
GLfloat lightDiffuse[]={1.0f,1.0f,1.0f,1.0f};
GLfloat lightPosition[]={0.0f,0.0f,20.0f,1.0f};

static void wheel(float rad,float width)
{
	int i;
	float colorswitch = 0;
	float clr=1.0;
	glColor3f(clr, clr, clr);
	glBegin(GL_QUAD_STRIP);
	for(i=0;i<=360;i+=10){
		if(colorswitch > 4) {
			colorswitch = 0;
			clr = !clr;
			glColor3f(clr, clr, clr);
		}
		else colorswitch++;
		glNormal3f(0,sin(DEGTORAD(i)),cos(DEGTORAD(i)));
		glVertex3f(0,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
		glVertex3f(width,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
	}
	glEnd();
	glColor3f(0.8, 0.8, 0.8);
	glBegin(GL_TRIANGLE_FAN);
	glNormal3f(1, 0, 0);
	glVertex3f(width+0.02,0,0);
	for(i=360;i>=0;i-=10){
		glNormal3f(0.707, 0.707*sin(DEGTORAD(i)), 0.707*cos(DEGTORAD(i)));
		glVertex3f(width,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
	}
	glEnd();
	glBegin(GL_TRIANGLE_FAN);
	glNormal3f(-1, 0, 0);
	glVertex3f(-0.02,0,0);
	for(i=0;i<=360;i+=10){
		glNormal3f(-0.707, 0.707*sin(DEGTORAD(i)),0.707*cos(DEGTORAD(i)));
		glVertex3f(0,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
	}
	glEnd();
}
static void tube(float rad,float width)
{
	int i;
	glBegin(GL_QUAD_STRIP);
	for(i=0;i<=360;i+=20){
		glNormal3f(0, sin(DEGTORAD(i)), cos(DEGTORAD(i)));
		glVertex3f(0,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
		glVertex3f(width,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
	}
	glEnd();
	glBegin(GL_TRIANGLE_FAN);
	glNormal3f(1, 0, 0);
	glVertex3f(width,0,0);
	for(i=360;i>=0;i-=20){
		glVertex3f(width,rad*sin(DEGTORAD(i)), rad*cos(DEGTORAD(i)));
	}
	glEnd();
}
static void hokuyoCase(void)
{
	glPushMatrix();
	glColor3f(0, 0, 0);
	glRotated(-90, 0, 1, 0);
	glTranslated(-0.016, 0, 0);
	tube(0.02,0.029);
	glColor3f(0, 1, 0);
	glBegin(GL_TRIANGLES);
	glNormal3f(1, 0, 0);
	glVertex3f(0.0295,0,-0.005);
	glVertex3f(0.0295,0.02,0);
	glVertex3f(0.0295,0,0.005);
	glEnd();
	glColor3f(1, 1, 1);
	glBegin(GL_QUAD_STRIP);
	glNormal3f(0, 0, 1);
	glVertex3f(-0.041, -0.025, 0.025);
	glVertex3f(0, -0.025, 0.025);
	glVertex3f(-0.041, 0.025, 0.025);
	glVertex3f(0, 0.025, 0.025);
	glNormal3f(0, 1, 0);
	glVertex3f(-0.041, 0.025, -0.025);
	glVertex3f(0, 0.025, -0.025);
	glNormal3f(0, 0, -1);
	glVertex3f(-0.041, -0.025, -0.025);
	glVertex3f(0, -0.025, -0.025);
	glNormal3f(0, -1, 0);
	glVertex3f(-0.041, -0.025, 0.025);
	glVertex3f(0, -0.025, 0.025);
	glEnd();
	glBegin(GL_QUADS);
	glNormal3f(1, 0, 0);
	glVertex3f(0,-0.025,0.025);
	glVertex3f(0,-0.025,-0.025);
	glVertex3f(0,0.025,-0.025);
	glVertex3f(0,0.025,0.025);
	glNormal3f(-1, 0, 0);
	glVertex3f(-0.041,-0.025,0.025);
	glVertex3f(-0.041,0.025,0.025);
	glVertex3f(-0.041,0.025,-0.025);
	glVertex3f(-0.041,-0.025,-0.025);
	glEnd();
	glPopMatrix();
}


@interface simController (Private)
- (void)initGL;
- (void)updateProjection;
- (void)initCamera;
- (void)updateCamera;
- (void)overlayFrame;
- (void)generateGround;
- (void)drawRover;
- (void)drawWaypoints;
- (void)positionUpdate;
- (void)bodyUpdate;
- (void)panelUpdate;
- (void)laserUpdate:(laserScanner*)scanner pointsList:(Point3D *)PL distanceData:(int *)distData;
- (void) drawFan:(laserScanner*)scanner;
- (int)intersectRayTriangle:(Ray3D)R triangle:(Triangle3D)T intersect:(Point3D*)I;
- (int)intersectRay:(Ray3D)R block:(Crate *)C intersect:(Point3D*)I;
- (float)heightAtPoint:(Point3D)pt;
- (Point3D)getWorldPointOnRoverFor:(Point3D)pt;
- (void)updateTools;
- (void)testblock;
@end


@implementation simController
+ (NSOpenGLPixelFormat*) basicPixelFormat
{
    NSOpenGLPixelFormatAttribute attributes [] = {
        NSOpenGLPFAWindow,
        NSOpenGLPFADoubleBuffer,	// double buffered
        NSOpenGLPFADepthSize, (NSOpenGLPixelFormatAttribute)16, // 16 bit depth buffer
        (NSOpenGLPixelFormatAttribute)nil
    };
    return [[[NSOpenGLPixelFormat alloc] initWithAttributes:attributes] autorelease];
}

- (id)initWithFrame:(NSRect)frameRect
{
	NSOpenGLPixelFormat * pf = [simController basicPixelFormat];
	self = [super initWithFrame: frameRect pixelFormat: pf];
	camView = 2;
	botPos.x = 0;
	botPos.y = 0;
	botPos.z = 0;
	leftEncoder = rightEncoder = 0;
	leftSpeed = rightSpeed = 0;
	loopTime = 1.0/30.0;
	leftWheelSpin = rightWheelSpin = 0;
	heading = 0;
	ticksPerMeter = (float)rk.LTICSPERREV/(PI*rk.WHEELDIAM);
	simBodyData = NULL;
	simPanelData = NULL;
	simProfileData = NULL;
	groundPlane = NULL;
	groundNormals = NULL;
	nGridSize = 100.0;
	nOffset = 4; 
	mGridSize = 100.0;
	mOffset = 4; 
	beamsOn = NO;
	WPList = NULL;
	[toolsPanel setFloatingPanel:NO];
	[toolsPanel setReleasedWhenClosed:NO];
	[toolsPanel close];
	blocks = [[NSMutableArray alloc] init];
	localBlocks = [[NSMutableArray alloc] init];
	return self;
}

- (void)prepareOpenGL
{
	GLint swapInt = 1;
	//set to vbl sync
	[[self openGLContext] setValues:&swapInt forParameter:NSOpenGLCPSwapInterval];
	//init GL stuff here
	glClearDepth(1.0f);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LEQUAL);
	glShadeModel(GL_SMOOTH);
	glClearColor(0,0,0,0.5);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	glLightfv(GL_LIGHT1,GL_AMBIENT,lightAmbient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, lightDiffuse);
	glLightfv(GL_LIGHT1, GL_POSITION, lightPosition);
	glEnable(GL_LIGHT1);
	//glEnable(GL_TEXTURE_2D);
	//glEnable(GL_COLOR_MATERIAL);
	//glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE); //LIGHT BACKSIDE OF SURFACES
	
	[self initCamera];
	
	[cursorPosition setStringValue:[NSString stringWithFormat:@"Crosshair (%0.3f,%0.3f)",camera.viewPosition.x,camera.viewPosition.y]];	
	
	timer = [NSTimer scheduledTimerWithTimeInterval:(loopTime) target:self selector:@selector(drawFrame) userInfo:nil repeats:YES];
	
	[[NSRunLoop currentRunLoop] addTimer:timer forMode:NSDefaultRunLoopMode];
	[[NSRunLoop currentRunLoop] addTimer:timer forMode:NSEventTrackingRunLoopMode];
	[self initGL];
}

- (void) reshape
{
	NSRect rectView = [self bounds];
	camera.viewHeight = rectView.size.height;
	camera.viewWidth = rectView.size.width;
	glViewport (0, 0, camera.viewWidth, camera.viewHeight);
	[self updateProjection];
}
- (void)updateProjection
{
	GLdouble ratio;
	[[self openGLContext] makeCurrentContext];
	
	// set projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	ratio = camera.viewWidth / (float) camera.viewHeight;
	gluPerspective(45.0, (GLfloat)ratio, NEARCLIPPING, FARCLIPPING);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}
- (void)initCamera
{
	camera.aperture = 40;
	camera.pitch = 0;
	camera.roll = 0;
	camera.yaw = 0;
	camera.zoom = 30.0;
	camera.viewPosition.x = 0.0;
	camera.viewPosition.y = 0.0;
	camera.viewPosition.z = 0.0;
	camera.viewDirection.x = 0.0; 
	camera.viewDirection.y = 0.0; 
	camera.viewDirection.z = 0.0;
	
	camera.viewUp.x = 0;  
	camera.viewUp.y = 1; 
	camera.viewUp.z = 0;
	
	gridStep = (int) camera.viewPosition.z/4;
	[mapScale setIntValue:gridStep];
}

- (void)generateGround
{
	int i,j,k;
	Vector3D baseNorm;
	
	if(groundPlane) free(groundPlane);
	vertexCount = (2*(nGridSize+1)*mGridSize);
	groundPlane = malloc(sizeof(Point3D)*vertexCount);
	groundNormals = malloc(sizeof(Vector3D)*vertexCount);
	
	k=0;
	baseNorm.x = 0;
	baseNorm.y = 0;
	baseNorm.z = 1;
	for(j=0;j<mGridSize;j++){
		for(i=0;i<(nGridSize+1);i++){
			k=i*2+j*2*(nGridSize+1);
			groundNormals[k] = baseNorm;
			groundPlane[k].x = ((float)i-nOffset)*GROUNDSCALE;
			groundPlane[k].y = ((float)j-mOffset)*GROUNDSCALE;
			groundPlane[k].z = 0;
			k++;
			groundNormals[k] = baseNorm;
			groundPlane[k].x = ((float)i-nOffset)*GROUNDSCALE;
			groundPlane[k].y = ((float)j+1-mOffset)*GROUNDSCALE;
			groundPlane[k].z = 0;
		}
	}
	[self randomizeObstacles];
}

- (void)initGL
{
	// wheel positions based on rover frame
	frontLeft = MakePoint(WHEELBASE/2, WHEELTRACK/2, 0);
	frontRight = MakePoint(WHEELBASE/2, -WHEELTRACK/2, 0);
	rearLeft = MakePoint(-WHEELBASE/2, WHEELTRACK/2, 0);
	rearRight = MakePoint(-WHEELBASE/2, -WHEELTRACK/2, 0);
	
	aWheel = glGenLists(1);
	glNewList(aWheel, GL_COMPILE);
	wheel(rk.WHEELDIAM/2.0,WHEELWIDTH);
	glEndList();
	
	suspension = glGenLists(1);
	glNewList(suspension, GL_COMPILE);
	tube(0.026, 0.24);
	glTranslated(0.22, 0, 0);
	glRotated(90, 0, 0, 1);
	glRotated(30, 0, 1, 0);
	tube(0.013, 0.37);
	glRotated(120, 0, 1, 0);
	tube(0.013, 0.37);
	glEndList();
	
	laserCase = glGenLists(1);
	glNewList(laserCase, GL_COMPILE);
	hokuyoCase();
	glEndList();
	
	[self generateGround];
}

- (void)drawFrame
{
	[self drawRect:[self frame]];
}
- (void)drawRect:(NSRect)bounds
{
	int i,j,k;
	static float gridSize = 1000.0;
	float	colorScale;
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();
	glClearColor(0.2,0.2,0.2,0);
	
	[self updateCamera];	// update the camera view
	
	// draw ground plane
	glEnable(GL_LIGHTING);
	glEnable(GL_COLOR_MATERIAL);
	glColor3f(0.1, 0.1, 0.4);
	for(j=0;j<mGridSize;j++){
		glBegin(GL_TRIANGLE_STRIP);
		for(i=0;i<2*(nGridSize+1);i++)
		{
			k = i+j*2*(nGridSize+1);
			glNormal3f(groundNormals[k].x, groundNormals[k].y, groundNormals[k].z);
			colorScale = 2*(groundPlane[k].z+HEIGHTSCALE)/HEIGHTSCALE;
			glColor3f(0.2*colorScale, 0.2*colorScale, 0.1*colorScale);
			glVertex3f(groundPlane[k].x, groundPlane[k].y, groundPlane[k].z);
		}
		glEnd();
	}
	// draw obstacle blocks
	for(j=0;j<[blocks count];j++)
	{
		[[blocks objectAtIndex:j] drawCrate];
	}
	glDisable(GL_LIGHTING);
	glDisable(GL_COLOR_MATERIAL);
	
	// draw grid
	if(0){
		glColor3f(0.5,0.5,1.0);
		for(i = 0; i <= gridSize; i += gridStep)
		{
			if(i == 0) glLineWidth(3.0);
			else glLineWidth(1.0);
			glBegin(GL_LINES);
			glVertex3f(-gridSize, i, 0.005);					
			glVertex3f(gridSize, i, 0.005);
			glVertex3f(i, -gridSize, 0.005);							
			glVertex3f(i, gridSize, 0.005);
			glEnd();
		}
		for(i = 0; i >= -gridSize; i -= gridStep)
		{
			glBegin(GL_LINES);
			glVertex3f(-gridSize, i, 0.005);					
			glVertex3f(gridSize, i, 0.005);
			glVertex3f(i, -gridSize, 0.005);							
			glVertex3f(i, gridSize, 0.005);
			glEnd();
		}
	}
	if(WPList != NULL) [self drawWaypoints];
	
	//////////////////////////////////////////////////////////////
	// besure to update the rover position before any calculations
	[self positionUpdate]; 
	
	[self laserUpdate:bodyLaser pointsList:BLPoints distanceData:simBodyData];
	[self laserUpdate:panelLaser pointsList:PLPoints distanceData:simPanelData];
	[self laserUpdate:profileLaser pointsList:PROLPoints distanceData:simProfileData];
	
	// draw rover
	[self drawRover];
	
	glLineWidth(1.0);
	[self overlayFrame];
	
	xbcTime += (int)(loopTime/0.001);
	[[self openGLContext] flushBuffer];
}
- (void)drawWaypoints
{
	wayPoint tempwp = *WPList;
	glPointSize(4.0);
	glColor3f(1, 1, 0);
	glBegin(GL_POINTS);
	while(1){
		glVertex3f(tempwp.position.x,tempwp.position.y,[self heightAtPoint:tempwp.position]+0.1);
		if(tempwp.next == NULL) break;
		else tempwp = *tempwp.next;
	}
	glEnd();
}
- (void)drawRover
{
	glEnable(GL_CULL_FACE);
	glEnable(GL_LIGHTING);
	glEnable(GL_COLOR_MATERIAL);
	glPushMatrix();
	glTranslated(botPos.x, botPos.y, botPos.z);
	glRotated(RADTODEG(heading), 0, 0, 1);
	glRotated(RADTODEG(-pitch), 0, 1, 0);
	glRotated(RADTODEG(-roll), 1, 0, 0);
	
	glPushMatrix();
	glTranslated(0, 0, 0.24);
	glRotated(-90, 0, 0, 1);
	glColor3f(0.5, 0.5, 0.5);
	glBegin(GL_QUAD_STRIP);
	glNormal3f(1, 0, 0);
	glVertex3f(0.177, -0.2286, 0.2);
	glVertex3f(0.177, -0.2286, 0);
	glVertex3f(0.177, 0.2286, 0.2);
	glVertex3f(0.177, 0.2286, 0);
	glNormal3f(0, 1, 0);
	glVertex3f(-0.177, 0.2286, 0.2);
	glVertex3f(-0.177, 0.2286, 0);
	glNormal3f(-1, 0, 0);
	glVertex3f(-0.177, -0.2286, 0.2);
	glVertex3f(-0.177, -0.2286, 0);
	glNormal3f(0, -1, 0);
	glVertex3f(0.177, -0.2286, 0.2);
	glVertex3f(0.177, -0.2286, 0);
	glEnd();
	glBegin(GL_QUADS);
	glNormal3f(0, 0, -1);
	glVertex3f(0.177, -0.2286, 0);
	glVertex3f(0.177, 0.2286, 0);
	glVertex3f(-0.177, 0.2286, 0);
	glVertex3f(-0.177, -0.2286, 0);
	glEnd();	
	glBegin(GL_QUADS);
	glNormal3f(0, 0, 1);
	glVertex3f(0.177, -0.2286, 0.2);
	glVertex3f(0.177, 0.2286, 0.2);
	glVertex3f(-0.177, 0.2286, 0.2);
	glVertex3f(-0.177, -0.2286, 0.2);
	glEnd();
	// suspension tubes
	glColor3f(0.8, 0.8, 0.8);
	glPushMatrix();
	glTranslated(0.0, 0.0, 0.05);
	glCallList(suspension);
	glPopMatrix();
	glPushMatrix();
	glTranslated(0.0, 0.0, 0.05);
	glRotated(180, 0, 0, 1);
	glCallList(suspension);
	glPopMatrix();
	glPopMatrix();
	
	// body laser
	glPushMatrix();
	glTranslated(bodyLaser->origin.x,bodyLaser->origin.y,bodyLaser->origin.z);
	glRotated(180, 0.5, 0.5, 0);
	glCallList(laserCase);
	[self drawFan:bodyLaser];
	glPopMatrix();
	
	// panel laser
	glPushMatrix();
	glTranslated(panelLaser->origin.x,panelLaser->origin.y,panelLaser->origin.z);
	glRotated(-90, 0, 0, 1);
	glRotated(-45, 1, 0, 0);
	glCallList(laserCase);
	[self drawFan:panelLaser];
	glPopMatrix();
	
	// profile laser
	glPushMatrix();
	glTranslated(profileLaser->origin.x,profileLaser->origin.y,profileLaser->origin.z);
	//glRotated([degslider floatValue],[xslider floatValue],[yslider floatValue],[zslider floatValue]);
	glRotated(180, 0, -0.5, 0.5);
	glCallList(laserCase);
	[self drawFan:profileLaser];
	glPopMatrix();

	// wheels
	glPushMatrix();
	glTranslated(rk.FOROFFSETWHEELS, 0.24, rk.WHEELDIAM/2.0);
	glRotated(90, 0, 0, 1);
	glRotated(leftWheelSpin, -1, 0, 0);
	glCallList(aWheel);
	glPopMatrix();
	glPushMatrix();
	glTranslated(rk.FOROFFSETWHEELS, -(0.24+WHEELWIDTH), rk.WHEELDIAM/2.0);
	glRotated(90, 0, 0, 1);
	glRotated(rightWheelSpin, -1, 0, 0);
	glCallList(aWheel);
	glPopMatrix();
	glPushMatrix();
	glTranslated(-rk.FOROFFSETWHEELS, -(0.24+WHEELWIDTH), rk.WHEELDIAM/2.0);
	glRotated(90, 0, 0, 1);
	glRotated(rightWheelSpin, -1, 0, 0);
	glCallList(aWheel);
	glPopMatrix();
	glPushMatrix();
	glTranslated(-rk.FOROFFSETWHEELS, 0.24, rk.WHEELDIAM/2.0);
	glRotated(90, 0, 0, 1);
	glRotated(leftWheelSpin, -1, 0, 0);
	glCallList(aWheel);
	glPopMatrix();
	
	glPopMatrix();
	glDisable(GL_COLOR_MATERIAL);
	glDisable(GL_LIGHTING);
	glDisable(GL_CULL_FACE);
}
- (void)drawFan:(laserScanner*)scanner
{
	int i;
	float x,y,hyp;
	if(beamsOn == YES){
		glColor3f(1.0,0.0,0.0);
		glLineWidth(1.0);
		glBegin(GL_LINES);
		{
			for(i=0;i<scanner->rayCount;i++)
			{
				hyp = scanner->rangeData[i];
				x = 4*(hyp/4095.0)*cos(scanner->alpha+(float)i*scanner->dAlpha);
				y = 4*(hyp/4095.0)*sin(scanner->alpha+(float)i*scanner->dAlpha);
				glVertex3f(x,y,0.0);
				glVertex3f(0.0,0.0,0.0);
			}
		}
		glEnd();
	}		
	
	glPointSize(3.0);
	glColor3f(0, 1, 0);
	glBegin(GL_POINTS);
	{
		for(i=0;i<scanner->rayCount;i++)
		{
			hyp = scanner->rangeData[i];
			x = 4*(hyp/4095.0)*cos(scanner->alpha+(float)i*scanner->dAlpha);
			y = 4*(hyp/4095.0)*sin(scanner->alpha+(float)i*scanner->dAlpha);
			glVertex3f(x,y,0.0);
		}
	}
	glEnd();
}
- (void)positionUpdate
{
	float leftVelocity = leftSpeed/ticksPerMeter;
	float rightVelocity = rightSpeed/ticksPerMeter;
	float newHeading;
	float vDiff = rightVelocity - leftVelocity;
	float wflh,wfrh,wrlh,wrrh,diffW1,diffW2;
	//Point3D heightPoint;
	
	// ENCODER UPDATES
	leftEncoder += (int)(leftSpeed*loopTime);
	leftWheelSpin = -(float)leftEncoder/rk.LTICSPERREV*360.0;
	rightEncoder += (int)(rightSpeed*loopTime);
	rightWheelSpin = -(float)rightEncoder/rk.RTICSPERREV*360.0;

	// WHEEL HEIGHTS UPDATE
	wflh = [self heightAtPoint:[self getWorldPointOnRoverFor:frontLeft]];
	wfrh = [self heightAtPoint:[self getWorldPointOnRoverFor:frontRight]];
	wrlh = [self heightAtPoint:[self getWorldPointOnRoverFor:rearLeft]];
	wrrh = [self heightAtPoint:[self getWorldPointOnRoverFor:rearRight]];
	botPos.z = (wflh+wfrh+wrlh+wrrh)/4;
	
	// PITCH UPDATE
	diffW1 = wfrh - wrrh; diffW2 = wflh - wrlh;
	pitch = (asin((diffW1)/WHEELBASE) + asin((diffW2)/WHEELBASE))/2;
	if(pitch != pitch){
		if(diffW1 > 0) pitch = PI/2;
		else pitch = -PI/2;
	}
	// ROLL UPDATE
	diffW1 = wfrh - wflh; diffW2 = wrrh - wrlh;
	roll = (asin((diffW1)/WHEELTRACK) + asin((diffW2)/WHEELTRACK))/2;
	if(roll != roll){
		if(diffW1 > 0) roll = PI/2;
		else roll = -PI/2;
	}
	/*glPointSize(3.0);
	glColor3f(1, 1, 0);
	glBegin(GL_POINTS);
	glVertex3f(heightPoint.x, heightPoint.y, wflh);
	glEnd();
	*/
	// HEADING UPDATE
	newHeading = heading + vDiff*loopTime/WHEELTRACK;
	
	if(vDiff != 0){
		botPos.x += (WHEELTRACK*(rightVelocity+leftVelocity)/(2*(vDiff)))*(sin(newHeading) - sin(heading));
		botPos.y -= (WHEELTRACK*(rightVelocity+leftVelocity)/(2*(vDiff)))*(cos(newHeading) - cos(heading));
	}
	else{
		botPos.x += leftVelocity*loopTime*cos(heading);
		botPos.y += leftVelocity*loopTime*sin(heading);
	}
	
	if(newHeading < 0) newHeading = TWOPI+newHeading;
	else if(newHeading > TWOPI) newHeading = newHeading-TWOPI;
	heading = newHeading;
	
	//botFrame.X = MakeVector(cos(heading), sin(heading), 0);
	//botFrame.Y = MakeVector(-sin(heading), cos(heading), 0);
	//botFrame.Z = MakeVector(0, 0, 1);
	//generateRotationFrame(&botFrame);
	
	botFrame.X = MakeVector(cos(heading)*cos(-pitch), 
							cos(heading)*sin(-pitch)*sin(-roll)-sin(heading)*cos(-roll), 
							cos(heading)*sin(-pitch)*cos(-roll)+sin(heading)*sin(-roll));
	botFrame.Y = MakeVector(sin(heading)*cos(-pitch), 
							sin(heading)*sin(-pitch)*sin(-roll)+cos(heading)*cos(-roll), 
							sin(heading)*sin(-pitch)*cos(-roll)-cos(heading)*sin(-roll));
	botFrame.Z = MakeVector(-sin(-pitch), cos(-pitch)*sin(-roll), cos(-pitch)*cos(-roll));
}
- (void)updateCamera
{
	float cyaw,cpitch,cheight;
	if(camera.zoom < 0.5) camera.zoom = 0.5;
	else if(camera.zoom > FARCLIPPING) camera.zoom = FARCLIPPING-2.0;
	//if(camera.pitch > 89) camera.pitch = 89;
	//else if(camera.pitch < 0) camera.pitch = 0;
	
	cyaw = camera.yaw;
	cpitch = camera.pitch;
	if(camView > 0){
		camera.viewDirection.x = botPos.x;
		camera.viewDirection.y = botPos.y;
		camera.viewDirection.z = botPos.z;
		if(camView == 2) 
		{
			cyaw = camera.yaw + RADTODEG(heading);
			cpitch = camera.pitch + RADTODEG(pitch);
		}
		
	}
	camera.viewPosition.x = camera.viewDirection.x + camera.zoom*sin(DEGTORAD(cpitch))*sin(DEGTORAD(cyaw));
	camera.viewPosition.y = camera.viewDirection.y - camera.zoom*sin(DEGTORAD(cpitch))*cos(DEGTORAD(cyaw));
	camera.viewPosition.z = camera.viewDirection.z + camera.zoom*cos(DEGTORAD(cpitch));
	//cheight = [self heightAtPoint:camera.viewPosition]+1.0;
	//if(camera.viewPosition.z < cheight) camera.viewPosition.z = cheight;
	
	camera.viewUp.x = -cos(DEGTORAD(cpitch))*sin(DEGTORAD(cyaw));
	camera.viewUp.y = cos(DEGTORAD(cpitch))*cos(DEGTORAD(cyaw));
	camera.viewUp.z = sin(DEGTORAD(cpitch));
	
	gluLookAt(camera.viewPosition.x
			  , camera.viewPosition.y
			  , camera.viewPosition.z
			  , camera.viewDirection.x
			  , camera.viewDirection.y
			  , camera.viewDirection.z
			  , camera.viewUp.x, camera.viewUp.y, camera.viewUp.z);
	
	int mod = (int) camera.zoom/4;
	if(camera.zoom < 8.0) gridStep = 1.0;
	else gridStep = mod;
	[mapScale setIntValue:gridStep];
}
- (void)overlayFrame
{
	glPushMatrix();
	glLoadIdentity();
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D(0, [self frame].size.width, 0, [self frame].size.height);
	glColor3f(0, 1, 0);
	glBegin(GL_LINES);
	glVertex2i([self frame].size.width/2-5,[self frame].size.height/2);
	glVertex2i([self frame].size.width/2+5,[self frame].size.height/2);
	glVertex2i([self frame].size.width/2,[self frame].size.height/2+5);
	glVertex2i([self frame].size.width/2,[self frame].size.height/2-5);
	glEnd();
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
}
- (void)laserUpdate:(laserScanner*)scanner pointsList:(Point3D *)PL distanceData:(int *)distData
{
	int i,j,k,l,m,n;
	int envx,envy,envw;
	Ray3D ray;
	Triangle3D tri;
	Point3D isect;
	int result;
	int row;
	double tempMag;
	
	ray.P0.x = dotVP(botFrame.X,scanner->origin) + botPos.x;
	ray.P0.y = dotVP(botFrame.Y,scanner->origin) + botPos.y;
	ray.P0.z = dotVP(botFrame.Z,scanner->origin) + botPos.z;
	
	
	envw = 12/GROUNDSCALE;
	envx = (int)(botPos.x*GROUNDSCALE + nOffset) - envw/2;
	if(envx < 0) envx = 0;
	else if(envx > nGridSize-envw) envx = nGridSize-envw;
	envy = (int)(botPos.y*GROUNDSCALE + mOffset) - envw/2;
	if(envy < 0) envy = 0;
	else if(envy > mGridSize-envw) envy = mGridSize-envw;

	row = 2*envw;
	// add local obstacles to check for ray intersection
	[localBlocks removeAllObjects];
	for(j=0;j<[blocks count];j++){
		Point3D tempLocal = [[blocks objectAtIndex:j] getPosition];
		// check for crate location inside of bounding envelope
		if(tempLocal.x >= envx && tempLocal.x <= envx+envw && tempLocal.y >= envy && tempLocal.y <= envy+envw)
		[localBlocks addObject:[blocks objectAtIndex:j]];
	}
	
	for(i=0;i<scanner->rayCount;i++){
		ray.P1.x = dotVP(botFrame.X, PL[i]) + botPos.x;
		ray.P1.y = dotVP(botFrame.Y, PL[i]) + botPos.y;
		ray.P1.z = dotVP(botFrame.Z, PL[i]) + botPos.z;
		distData[i] = 4095;
		
		// check all terrain triangles for intersections
		for(m=0;m<envw;m++){
			l = m*2*(nGridSize+1) + envy*2*(nGridSize+1) + envx*2;
			for(n=0;n<row;n++){
				k=n+l;
				tri.V0 = groundPlane[k];
				tri.V2 = groundPlane[k+2];
				tri.V1 = groundPlane[k+1];
				
				/*glColor3f(0.4, 0, 0);
				glBegin(GL_TRIANGLES);
				glVertex3f(tri.V0.x, tri.V0.y, tri.V0.z+0.01);
				glVertex3f(tri.V1.x, tri.V1.y, tri.V1.z+0.01);
				glVertex3f(tri.V2.x, tri.V2.y, tri.V2.z+0.01);
				glEnd();
				*/
				result = [self intersectRayTriangle:ray triangle:tri intersect:&isect];
				if(result==1){
					tempMag = magnitude3d(ray.P0, isect)*1000.0;
					if(tempMag < (float)distData[i]) distData[i] = tempMag;
					//n=row+1;
					//m=envw;
				}
			}
		}
		for(j=0;j<[localBlocks count];j++){
			result = [self intersectRay:ray block:[localBlocks objectAtIndex:j] intersect:&isect];
			if(result==1){
				tempMag = magnitude3d(ray.P0, isect)*1000.0;
				if(tempMag < (float)distData[i]) distData[i] = tempMag;
			}
		}
	}
}

- (int)intersectRayTriangle:(Ray3D)R triangle:(Triangle3D)T intersect:(Point3D*)I
{
    Vector3D    u, v, n;             // triangle vectors
    Vector3D    dir, w0, w;          // ray vectors
    float     r, a, b;             // params to calc ray-plane intersect
	
    // get triangle edge vectors and plane normal
    pointsToVector(T.V1,T.V0,&u);
    pointsToVector(T.V2,T.V0,&v);
	cross(u, v, &n);				// cross product
    if (n.x==0 && n.y==0 && n.z==0)	// triangle is degenerate
        return -1;					// do not deal with this case
	
    pointsToVector(R.P1, R.P0, &dir);	// ray direction vector
    pointsToVector(R.P0, T.V0, &w0);
    a = -dot(n,w0);
    b = dot(n,dir);
    if (fabs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
        if (a == 0)                // ray lies in triangle plane
            return 2;
        else return 3;             // ray disjoint from plane
    }
	
    // get intersect point of ray with triangle plane
    r = a / b;
	// ray goes away from triangle or segment does not intersect => no intersect
    if (r < 0.0 || r > 1.0) return 0;
	
    I->x = R.P0.x - r * dir.x;           // intersect point of ray and plane
	I->y = R.P0.y - r * dir.y;
	I->z = R.P0.z - r * dir.z;
	
    // is I inside T?
    float    uu, uv, vv, wu, wv, D;
    uu = dot(u,u);
    uv = dot(u,v);
    vv = dot(v,v);
    pointsToVector(*I, T.V0, &w);
    wu = dot(w,u);
    wv = dot(w,v);
    D = uv * uv - uu * vv;
	
    // get and test parametric coords
    float s, t;
    s = (uv * wv - vv * wu) / D;
    if (s <= 0.0 || s > 1.0)        // I is outside T
        return 0;
    t = (uv * wu - uu * wv) / D;
    if (t <= 0.0 || (s + t) > 1.0)  // I is outside T
        return 0;
	
    return 1;                      // I is in T
}
// for testing block intersection code
- (void)testblock
{
	int j;
	Ray3D ray;
	Point3D isect;
	int result;
	//double tempMag;
	
	ray.P0.x = dotVP(botFrame.X,bodyLaser->origin) + botPos.x;
	ray.P0.y = dotVP(botFrame.Y,bodyLaser->origin) + botPos.y;
	ray.P0.z = dotVP(botFrame.Z,bodyLaser->origin) + botPos.z;
	
	ray.P1.x = dotVP(botFrame.X, MakePoint(4, 0, 0)) + botPos.x;
	ray.P1.y = dotVP(botFrame.Y, MakePoint(4, 0, 0)) + botPos.y;
	ray.P1.z = dotVP(botFrame.Z, MakePoint(4, 0, 0)) + botPos.z;
	
	for(j=0;j<[blocks count];j++){
		result = [self intersectRay:ray block:[blocks objectAtIndex:j] intersect:&isect];
		//if(result==1){
		//	tempMag = magnitude3d(ray.P0, isect)*1000.0;
		//	if(tempMag < distData[i]) distData[i] = tempMag;
		//}
	}
	glBegin(GL_LINES);
		glVertex3f(ray.P0.x, ray.P0.y, ray.P0.z);
		glVertex3f(ray.P1.x, ray.P1.y, ray.P1.z);
	glEnd();
	glBegin(GL_POINTS);
	glVertex3f(isect.x, isect.y, isect.z);
	glEnd();
}
// looks for intersection with retangular blocks returns 1 if there is a valid intersection
- (int)intersectRay:(Ray3D)R block:(Crate *)C intersect:(Point3D*)I
{
	int i,j;
	int			result = 0;
	Vector3D	*normArray;
	Point3D		*blockVertex,P;
    Vector3D    n, v, v1, v2;     // vectors
    Vector3D    u, w;          // ray vectors
    float		r, a, b;          // params to calc ray-plane intersect
	
	normArray = [C getNorms];
	blockVertex = [C getCorners];
	*I=R.P1;
	for(i=0;i<6;i++){
		// get plane normal
		n = normArray[i];
		j = 4*i;
		
		pointsToVector(R.P0, R.P1, &u);	// ray direction vector
		pointsToVector(blockVertex[j], R.P0, &w); // vector from ray base to corner
		a = -dot(n,w);
		b = dot(n,u);
		
		if (fabs(b) < SMALL_NUM) continue;     // ray is parallel to or disjoint from plane
		
		// get intersect point of ray with plane
		r = a / b;
		//NSLog(@"%d a %f b %f r %f",i,a,b,r);
		// ray goes away from plane or segment does not intersect => no intersect
		if (r < 0.0 || r > 1.0 || a>0 && b>0) continue;
		
		P.x = R.P0.x + r * u.x;          // intersect point of ray and plane
		P.y = R.P0.y + r * u.y;
		P.z = R.P0.z + r * u.z;
		
		// is I inside plane?
		pointsToVector(P, blockVertex[j], &v); // vector point to rectangle corner
		pointsToVector(blockVertex[j], blockVertex[j+1], &v1);	// rectangle side vectors
		pointsToVector(blockVertex[j], blockVertex[j+3], &v2);
		
		float aa,bb,cc,dd;
		aa=-dot(v,v1);
		bb=dot(v1,v1);
		cc=-dot(v,v2);
		dd=dot(v2,v2);
		
		if(0<=aa && aa<=bb && 0<=cc && cc<=dd){
			result = 1; // I is in the rectangle
			*I=P;
			break;
		}
	}
	return result;
}
- (Point3D)getWorldPointOnRoverFor:(Point3D)pt
{
	Point3D P0;
	P0.x = dotVP(botFrame.X,pt) + botPos.x;
	P0.y = dotVP(botFrame.Y,pt) + botPos.y;
	P0.z = dotVP(botFrame.Z,pt) + botPos.z;
	return P0;
}
- (float)heightAtPoint:(Point3D)pt
{
	int vertx,verty,vertexNumber;
	float height;
	float ztest;
	Point3D V0,V1,V2;
	Vector3D N;
	
	vertx = (int)(pt.x*GROUNDSCALE + nOffset);
	if(vertx < 0) vertx = 0;
	else if(vertx > nGridSize-1) vertx = nGridSize-1;
	verty = (int)(pt.y*GROUNDSCALE + mOffset);
	if(verty < 0) verty = 0;
	else if(verty > mGridSize-1) verty = mGridSize-1;
	
	vertexNumber = 2*vertx + 2*(nGridSize+1)*verty;
	
	V0 = groundPlane[vertexNumber];
	V1 = groundPlane[vertexNumber+1];
	V2 = groundPlane[vertexNumber+2];
	N = groundNormals[vertexNumber];
	
	ztest = (pt.x-V1.x)*(V2.y-V1.y)-(pt.y-V1.y)*(V2.x-V1.x);
	if(ztest<0) {
		V0 = groundPlane[vertexNumber+3];
		N = groundNormals[vertexNumber+1];
	}
	
	if(N.z != 0) height = (N.x*(pt.x-V0.x)+N.y*(pt.y-V0.y))/-N.z+V0.z;
	else height = V0.z;
	
	return height;
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Responder functions and user input
- (BOOL)acceptsFirstResponder
{
	return YES;
}
- (BOOL)becomeFirstResponder
{
	return YES;
}
- (BOOL)resignFirstResponder
{
	return YES;
}
- (void)mouseDown:(NSEvent*)theEvent
{
	mousePoint = [self convertPoint:[theEvent locationInWindow] fromView:nil];
}
- (void)mouseDragged:(NSEvent*)theEvent
{
	NSPoint pt = [self convertPoint:[theEvent locationInWindow] fromView:nil];
	if([theEvent modifierFlags] & NSControlKeyMask)
	{
		camera.zoom += camera.zoom*(mousePoint.y - pt.y)*0.005;
	}
	else if([theEvent modifierFlags] & NSCommandKeyMask)
	{
		camera.pitch += (pt.y - mousePoint.y)*0.1;
		camera.yaw += (mousePoint.x - pt.x)*0.1;
	}
	else if(camView == 0){
		camera.viewDirection.x += (cos(DEGTORAD(camera.yaw))*(mousePoint.x - pt.x)-sin(DEGTORAD(camera.yaw))*(mousePoint.y - pt.y))*camera.viewPosition.z*0.0025;
		camera.viewDirection.y += (cos(DEGTORAD(camera.yaw))*(mousePoint.y - pt.y)+sin(DEGTORAD(camera.yaw))*(mousePoint.x - pt.x))*camera.viewPosition.z*0.0025;
		[cursorPosition setStringValue:[NSString stringWithFormat:@"Crosshair (%0.3f,%0.3f)",camera.viewDirection.x,camera.viewDirection.y]];
	}
	mousePoint = pt;
}
- (void)scrollWheel:(NSEvent*)theEvent
{
	float scrl = [theEvent deltaY];
	if(scrl != 0.0){
		camera.zoom += camera.zoom*scrl*0.1;
	}
}
- (void)keyDown:(NSEvent*)event
{
	
	unichar keyChar = 0;
	if([event modifierFlags] & NSNumericPadKeyMask){
		NSString *theArrow = [event charactersIgnoringModifiers];
		
		if([theArrow length] == 0) return;
		if([theArrow length] == 1){
			keyChar = [theArrow characterAtIndex:0];
			switch (keyChar) {
				case NSUpArrowFunctionKey:
					leftSpeed+=(float)rk.JOYINCREMENT;
					rightSpeed+=(float)rk.JOYINCREMENT;
					break;
				case NSDownArrowFunctionKey:
					leftSpeed-=(float)rk.JOYINCREMENT;
					rightSpeed-=(float)rk.JOYINCREMENT;
					break;
				case NSLeftArrowFunctionKey:
					leftSpeed-=(float)rk.JOYINCREMENT;
					rightSpeed+=(float)rk.JOYINCREMENT;
					break;
				case NSRightArrowFunctionKey:
					leftSpeed+=(float)rk.JOYINCREMENT;
					rightSpeed-=(float)rk.JOYINCREMENT;
					break;
				default:
					break;
			}
		}
	}
	else{
		NSString *keyStroke = [event charactersIgnoringModifiers];
		if([keyStroke length] == 0) return;
		if([keyStroke length] == 1){
			keyChar = [keyStroke characterAtIndex:0];
			switch(keyChar){
				case ' ':
					leftSpeed = 0;
					rightSpeed = 0;
					break;
				case 'v':
					camView++;
					if(camView>2)camView = 0;
					break;
				case 'r':
					[self randomizeObstacles];
					break;
			}
		}
	}
	
}

- (void)setWPList:(wayPoint*)wp
{
	WPList = wp;
}
// old set Waypoint list setup, sets terrain length and width to encompass all waypoints
/*- (void)setWPList:(wayPoint*)wp
{
	WPList = wp;
	int farx,fary;
	wayPoint tempwp = *wp;
	
	farx = fary = 0;
	while(1){
		if((int)fabs(tempwp.position.x) > farx) farx = (int)tempwp.position.x;
		if((int)fabs(tempwp.position.y) > fary) fary = (int)tempwp.position.y;
		if(tempwp.next == NULL) break;
		tempwp = *tempwp.next;
	}
	
	if(farx < -100){
		nGridSize = fabs(farx) + 50;
		nOffset = fabs(farx) + 25;
	}
	else if(farx > 100){
		nGridSize = fabs(farx) + 50;
		nOffset = 25;
	}
	else{
		nGridSize = 100;
		nOffset = 50;
	}
	
	if(fary < -100){
		mGridSize = fabs(fary) + 50;
		mOffset = fabs(fary) + 25;
	}
	else if(fary > 100){
		mGridSize = fabs(fary) + 50;
		mOffset = 25;
	}
	else{
		mGridSize = 100;
		mOffset = 50;
	}
	
	[self generateGround];
}*/

- (float)pitch
{
	return RADTODEG(pitch);
}
- (float)roll
{
	return RADTODEG(roll);
}
- (float)heading
{
	float compHeading;
	// convert to compass heading instead of cartesian coordinates
	compHeading = (360-RADTODEG(heading)) + 90;
	if(compHeading > 360) compHeading -= 360;
	return compHeading;
}

- (int)xbcTime
{
	return xbcTime;
}
- (int)leftEncoder
{
	return leftEncoder;
}
- (int)rightEncoder
{
	return rightEncoder;
}
- (void)setSpeedLeft:(float)ls right:(float)rs
{
	leftSpeed = ls;
	rightSpeed = rs;
}

- (void)bodyLaserUpdate
{
	int i;
	for(i=0;i<bodyLaser->rayCount;i++){
		bodyLaser->rangeData[i] = simBodyData[i];
	}
}
- (void)panelLaserUpdate
{
	int i;
	for(i=0;i<panelLaser->rayCount;i++){
		panelLaser->rangeData[i] = simPanelData[i];
	}
}
- (void)profileLaserUpdate
{
	int i;
	for(i=0;i<profileLaser->rayCount;i++){
		profileLaser->rangeData[i] = simProfileData[i];
	}
}

- (Point3D*)calculateLaserPoints:(laserScanner*)ls
{
	int i;
	float laserRadius = 4.0;
	Point3D *tempPoints;
	Point3D	point;
	tempPoints = malloc(sizeof(Point3D)*ls->rayCount);
	
	// calculate laser points based on laser reference
	for(i=0;i<ls->rayCount;i++){ 
		point.x = laserRadius*cos(i*ls->dAlpha+ls->alpha);
		point.y = laserRadius*sin(i*ls->dAlpha+ls->alpha);
		point.z = 0;
		//NSLog(@"%d= %f,%f,%f",i,point.x,point.y,point.z);
	// move the above points to robot reference frame
		tempPoints[i].x = dotVP(ls->frame.X,point) + ls->origin.x;
		tempPoints[i].y = dotVP(ls->frame.Y,point) + ls->origin.y;
		tempPoints[i].z = dotVP(ls->frame.Z,point) + ls->origin.z;
		//NSLog(@"%d= %f,%f,%f",i,tempPoints[i].x,tempPoints[i].y,tempPoints[i].z);
	}
	return tempPoints;
}

- (void)getLasers:(laserScanner*)body panel:(laserScanner*)panel profile:(laserScanner*)profile
{
	bodyLaser = body;
	bodyLaser->origin = MakePoint(rk.FOROFFSETBODYLASER, 0, 0.25);	// get the origin of the laser scanner based on rover coordinates
	bodyLaser->frame.X = MakeVector(0, 1, 0); //get the laser frame to rover frame rotational vectors
	bodyLaser->frame.Y = MakeVector(1, 0, 0);
	bodyLaser->frame.Z = MakeVector(0, 0, -1);
	generateRotationFrame(&bodyLaser->frame);
	BLPoints = [self calculateLaserPoints:bodyLaser]; // calculate the endpoints of the laser rays in rover frame
	
	panelLaser = panel;
	panelLaser->origin = MakePoint(rk.FOROFFSETPANELLASER, 0, rk.PANELHEIGHT);
	panelLaser->frame.X = MakeVector(0, -1, 0);
	panelLaser->frame.Y = MakeVector(cos(DEGTORAD(-45)), 0, sin(DEGTORAD(-45)));
	panelLaser->frame.Z = MakeVector(-sin(DEGTORAD(-45)), 0, cos(DEGTORAD(-45)));
	generateRotationFrame(&panelLaser->frame);
	PLPoints = [self calculateLaserPoints:panelLaser];
	
	profileLaser = profile;
	profileLaser->origin = MakePoint(rk.FOROFFSETPROFILE, -0.1, rk.PROFILEHEIGHT);
	profileLaser->frame.X = MakeVector(-1, 0, 0);
	profileLaser->frame.Y = MakeVector(0, 0, -1);
	profileLaser->frame.Z = MakeVector(-1, 0, 0);
	generateRotationFrame(&profileLaser->frame);
	PROLPoints = [self calculateLaserPoints:profileLaser];
	
	// create memory to hold the distance calculations for the lasers
	if(simBodyData)free(simBodyData);
	if(simPanelData)free(simPanelData);
	if(simProfileData)free(simProfileData);
	simBodyData = malloc(sizeof(int)*bodyLaser->rayCount);
	simPanelData = malloc(sizeof(int)*panelLaser->rayCount);
	simProfileData = malloc(sizeof(int)*profileLaser->rayCount);
	bodyLaser->rangeData = simBodyData;
	panelLaser->rangeData = simPanelData;
	profileLaser->rangeData = simProfileData;
}

- (void)updateTools
{
	[d setFloatValue:[degslider floatValue]];
	[xs setFloatValue:[xslider floatValue]];
	[ys setFloatValue:[yslider floatValue]];
	[zs setFloatValue:[zslider floatValue]];
}
- (IBAction)showTools:(id)sender
{
	[toolsPanel makeKeyAndOrderFront:self];
}
- (void)drawLaserBeams:(BOOL)q
{
	beamsOn = q;
}

/////////////////////////////////////////////////////////////////
// Open a bitmap map file for world generation
- (IBAction)openFile:(id)sender
{
	NSOpenPanel *fileOpen = [NSOpenPanel openPanel];
	[fileOpen runModalForDirectory:[@"~/Desktop/" stringByExpandingTildeInPath] file:@""];
	if([[fileOpen filename] hasSuffix:@".bmp"]) [self loadTerrainFromFile:[fileOpen filename]];
}

- (void)loadTerrainFromFile:(NSString*)filename
{
	int iWidth,iHeight,iOffsetData;
	int i,j,k,tHeight;
	float **heightArray;
	short int iPixelDepth;
	
	NSFileHandle *bitmap = [NSFileHandle fileHandleForReadingAtPath:filename];
	[bitmap seekToFileOffset:10];
	[[bitmap readDataOfLength:4] getBytes:&iOffsetData];
	[bitmap seekToFileOffset:18];
	[[bitmap readDataOfLength:4] getBytes:&iWidth];
	[bitmap seekToFileOffset:22];
	[[bitmap readDataOfLength:4] getBytes:&iHeight];
	[bitmap seekToFileOffset:28];
	[[bitmap readDataOfLength:2] getBytes:&iPixelDepth];
	iPixelDepth = (int)((float)iPixelDepth/8.0);
	[bitmap	seekToFileOffset:iOffsetData];
	
	if(groundPlane) free(groundPlane);
	nGridSize = (float)fabs(iWidth)-1;
	mGridSize = (float)fabs(iHeight)-1;
	
	vertexCount = (2*(nGridSize+1)*mGridSize);
	groundPlane = malloc(sizeof(Point3D)*vertexCount);
	
	heightArray = malloc(sizeof(float *)*nGridSize+1);
	for(i=0;i<nGridSize+1;i++) heightArray[i] = malloc(sizeof(float)*mGridSize+1);
	for(j=0;j<mGridSize+1;j++){
		for(i=0;i<(nGridSize+1);i++){
			tHeight = 0;
			[[bitmap readDataOfLength:iPixelDepth] getBytes:&tHeight];
			heightArray[i][j] = ((float)(tHeight - 8388608)/16777216)*HEIGHTSCALE;
		}
	}
	
	k=0;
	for(j=0;j<mGridSize;j++){
		for(i=0;i<(nGridSize+1);i++){
			k=i*2+j*2*(nGridSize+1);
			
			groundPlane[k].x = ((float)i-nOffset)*GROUNDSCALE;
			groundPlane[k].y = ((float)j-mOffset)*GROUNDSCALE;
			groundPlane[k].z = heightArray[i][(int)mGridSize-j];
			k++;
			groundPlane[k].x = ((float)i-nOffset)*GROUNDSCALE;
			groundPlane[k].y = ((float)j+1-mOffset)*GROUNDSCALE;
			groundPlane[k].z = heightArray[i][(int)mGridSize-(j+1)];
		}
	}
	for(i=0;i<nGridSize+1;i++) free(heightArray[i]);
	free(heightArray);
	
	if(groundNormals) free(groundNormals);
	groundNormals = malloc(sizeof(Vector3D)*vertexCount);
	Vector3D tempV1,tempV2;
	for(j=0;j<mGridSize;j++){
		for(i=0;i<2*(nGridSize+1);i++){
			k=i+j*2*(nGridSize+1);
			
			if(i>=2*nGridSize) groundNormals[k]=groundNormals[k-1];
			else {
				pointsToVector(groundPlane[k], groundPlane[k+2], &tempV1);
				pointsToVector(groundPlane[k], groundPlane[k+1], &tempV2);
				if(NormalizeCross(&tempV1,&tempV2,&groundNormals[k]) < 0){
					groundNormals[k].x *=-1;
					groundNormals[k].y *=-1;
					groundNormals[k].z *=-1;
				}
			}
		}
	}
	[self randomizeObstacles];
}

- (void)randomizeObstacles
{
	int i;
	Crate *tempCrate;
	Point3D tempPoint;
	
	[blocks removeAllObjects];
	
	for(i=0;i<500;i++){
		tempPoint.x = 3+Randomn()*80;
		tempPoint.y = 3+Randomn()*80;
		tempCrate = [[Crate alloc] initCrateAlpha:Randomn()*HALFPI
												X:tempPoint.x
												Y:tempPoint.y
												Z:[self heightAtPoint:tempPoint]
												L:0.1+Randomn()*MAXL
												W:0.1+Randomn()*MAXW
												H:0.25+Randomn()*MAXH];
		[tempCrate setColorR:0.2 G:0.1 B:0.1 A:1];
		[blocks addObject:tempCrate];
		[tempCrate release];
	}
}
	
@end
